%%%%%%%%
%这是一个使用EKF进行多普勒定位的示例
%by Zichen 2024
%%%%%%%%%%%%%
%% 准备仿真数据
clear;clc
%数据别忘了配置自己的路径
load('experimental_data.mat')

sat.satpos_sigma = 10; %设置卫星位置误差
sat.satvel_sigma = 0.1;  %设置卫星速度误差
sat = sat.add_satpos_res;  %添加误差到sat对象
my_tra2 = my_trajectory(28.2213,112.9913,20,sat.timerange,7); %生成轨迹
startpos = lla2ecef([28.2213,112.9913,20]);
sensors_dynamic = sensor_body_dynamic(my_tra2);

sensors_dynamic.receiver_clock_drift = 10^-11; %单位是s/s  差不多是10^-6 ppm 百万秒稳
sensors_dynamic.angle_mask = 5;%设置仰角屏蔽值
sensors_dynamic = sensors_dynamic.get_sat_angle(sat);
sensors_dynamic.noise_obs = 1;
sensors_dynamic = sensors_dynamic.get_doppler_obs(sat);

sensors_dynamic = sensors_dynamic.Doppler_position_using_EKF([startpos,0,0,0,0]);
jieguo = {sensors_dynamic.EKF.S}';
[pos,vel] = sensors_dynamic.error_analysis...
    ("ned",jieguo,1);

